#include "algoritmos.h"

vector<float> recuperarColumna(Matriz& ,int);
float norma2(vector<float> &);
Matriz calcularMatrizW(vector<float>&, int);
Matriz calcularMatrizG(Matriz&, int, int);
void mostrarMatriz(Matriz&);

Matriz calcularQRReflexiones(Matriz& A){
    Matriz Q = calcularQRReflexiones(A, false);
    return Q;
}
Matriz calcularQRReflexiones(Matriz& A, bool quieroIteraciones){    // Devuelve Q y queda en la matriz input R
    //Matriz A = X;
    Matriz Q(A.size(),A.size());
    //cout << endl << endl;
    int iteraciones = 0;
    for(int k = 1; k <= A.size() - 1; k++){
        vector<float> x = recuperarColumna(A, k);      // x
        x[0] = x[0] - norma2(x);                        // u = x - alpha*e1
        float normaX = norma2(x);
        for(unsigned int i = 0; i < x.size(); i++){              // v = u/|u|
            x[i] = x[i]/normaX;
        }
        Matriz W = calcularMatrizW(x, A.size());

        A = W.multiplicar(A);
        iteraciones++;

        if(k == 1){
            Q = W;
        }else{
            Q = Q.multiplicar(W);
        }

    }
    if(quieroIteraciones)cout << iteraciones << endl;
    return Q;
}

vector<float> recuperarColumna(Matriz& A,int k){
    //cout << "Recuperando columna: " << k << endl;
    vector<float> x;
    for(int i = k; i <= A.size(); i++){
        x.push_back(A.get(i,k));
    }
    return x;
}

float norma2(vector<float> & x){
    float suma = 0;
    for(unsigned int i = 0; i < x.size(); i++){
        suma = suma + pow(x[i],2);
    }
    suma = sqrt(suma);
    return suma;
}

Matriz calcularMatrizW(vector<float>& x, int m){
    unsigned int n = (unsigned int) m;
    Matriz xxt(x.size(), x.size());

    for(int i = 1; i <= xxt.size(); i++){       // Creo la matriz 2 * (x * x(traspueto))
        for(int j = 1; j <= xxt.size(); j++){
            xxt.set(i, j, 2*x[i-1]*x[j-1]);
        }
    }

    Matriz I(x.size(), x.size());
    for(int i = 1; i <= xxt.size(); i++){       // Creo la matriz identidad
        for(int j = 1; j <= xxt.size(); j++){
            if(i == j){
                I.set(i, j, 1);
            }else{
                I.set(i, j, 0);
            }
        }
    }

    Matriz W(x.size(), x.size());
    for(int i = 1; i <= xxt.size(); i++){       // Calculo W = I - 2*x*x(transpuesto)
        for(int j = 1; j <= xxt.size(); j++){
            W.set(i, j, I.get(i,j) - xxt.get(i,j));
        }
    }
    Matriz result(n, n);

    for(unsigned int i = 1; i <= n; i++){       // Creo la matriz 2 * (x * x(traspueto))
        for(unsigned int j = 1; j <= n; j++){
            if(i <= n - x.size() || j <= n - x.size()){
                if(i == j){
                    result.set(i, j, 1);
                }else{
                    result.set(i, j, 0);
                }
            }else{
                result.set(i, j, W.get(i - (n - x.size()), j - (n - x.size())));
            }
        }
    }

    return result;
}

Matriz calcularQRRotaciones(Matriz& A){
    Matriz Q = calcularQRRotaciones(A, false);
    return Q;
}
Matriz calcularQRRotaciones(Matriz& A, bool quieroIteraciones){     // Devuelve Q y queda en la matriz inpu R
    Matriz Q(A.size(), A.size());
    int iteraciones = 0;
    for(int i = 1; i <= A.size(); i++){       // Seteo a Q con la identidad
        for(int j = 1; j <= A.size(); j++){
            if(i == j){
                Q.set(i, j, 1);
            }else{
                Q.set(i, j, 0);
            }
        }
    }

    for(int i = 1; i <= A.size(); i++){     // recorro columnas
        for(int j = i + 1; j <= A.size(); j++){         // recorro filas
            if(A.get(j,i) != 0){
                Matriz G = calcularMatrizG(A, j, i);
                cout << "Matriz G" << endl;
                mostrarMatriz(G);
                A = G.multiplicar(A);
                Q = G.multiplicar(Q);
                iteraciones ++;
            }
        }
    }
    Matriz QT = Q.trasponer();
    if(quieroIteraciones)cout << iteraciones << endl;
    return QT;

}

Matriz calcularMatrizG(Matriz& A, int fil, int col){

    Matriz G(A.size(), A.size());
    vector<float> x;
    x.push_back(A.get(col, col));
    x.push_back(A.get(fil, col));
    float norma2X = norma2(x);
    for(int i = 1; i <= A.size(); i++){
        for(int j = 1; j <= A.size(); j++){
            if(i == fil){
                if(j == fil){   // Arriba a la izquierda
                    G.set(i, j, x[0]/norma2X);
                }else{
                    if(j == col){  // Arriba a la derecha
                        G.set(i, j, (-1) * x[1]/norma2X);
                    }
                }
            }else{
                if(i == col){
                    if(j == fil){   // Abajo a la izquierda
                        G.set(i, j, x[1]/norma2X);
                    }else{
                        if(j == col){   // Abajo a la derecha
                            G.set(i, j, x[0]/norma2X);
                        }
                    }
                }else{  // La identidad
                    if(i == j){
                        G.set(i, j, 1);
                    }else{
                        G.set(i,j, 0);
                    }
                }
            }
        }
    }
    return G;
}

void mostrarMatriz(Matriz& A){
    for(int i = 1; i <= A.size(); i++){
        for(int j = 1; j <= A.size(); j++){
            cout << A.get(i,j) << " ";
        }
        cout << endl;
    }
    cout << endl;
}
